/*
 * Copyright (c) 2006-2024, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2024-07-22     RT-Thread    first version
 */

#include <rtthread.h>
#include "board.h"
#define DBG_TAG "main"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#include "mpu6xxx.h"
#include "mpu6050.h"
#include "uart.h"

#include "at.h"
#include <math.h>

#include <stdio.h>
#include <stdbool.h>

#define FILTER_COEFFICIENT 0.8 // 滤波系数α

#define FILTER_WINDOW_SIZE 5 // 滑动窗口大小

// 假定以下函数和结构体已定义
typedef struct
{
    float ax, ay, az; // 线性加速度
    float gx, gy, gz; // 重力加速度
} AccelerationData;

typedef struct
{
    float buffer[FILTER_WINDOW_SIZE]; // 滤波器缓冲区
    int index;                        // 缓冲区索引
} FilterBuffer;

#define MPU6XXX_DEVICE_NAME "i2c2"

// 初始化函数，用于初始化重力加速度值
void init_acceleration(AccelerationData *acceleration)
{
    acceleration->gx = 0.0;
    acceleration->gy = 0.0;
    acceleration->gz = 0.0;
}

// 加速度分解函数
void decompose_acceleration(float adx, float ady, float adz,
                            AccelerationData *acceleration,
                            bool isFirstSample)
{
    if (isFirstSample)
    {
        acceleration->ax = adx;
        acceleration->ay = ady;
        acceleration->az = adz;
    }
    else
    {
        acceleration->gx = FILTER_COEFFICIENT * adx + (1 - FILTER_COEFFICIENT) * acceleration->gx;
        acceleration->gy = FILTER_COEFFICIENT * ady + (1 - FILTER_COEFFICIENT) * acceleration->gy;
        acceleration->gz = FILTER_COEFFICIENT * adz + (1 - FILTER_COEFFICIENT) * acceleration->gz;

        acceleration->ax = adx - acceleration->gx;
        acceleration->ay = ady - acceleration->gy;
        acceleration->az = adz - acceleration->gz;
    }
}

// 初始化滤波器缓冲区
void init_filter_buffer(FilterBuffer *filter)
{
    for (int i = 0; i < FILTER_WINDOW_SIZE; i++)
    {
        filter->buffer[i] = 0.0;
    }
    filter->index = 0;
}

// 数据预处理函数
float preprocess_data(float data, FilterBuffer *filter)
{
    filter->buffer[filter->index % FILTER_WINDOW_SIZE] = data;
    filter->index++;

    float sum = 0.0;
    for (int i = 0; i < FILTER_WINDOW_SIZE; i++)
    {
        sum += filter->buffer[i];
    }

    return sum / FILTER_WINDOW_SIZE;
}

int main(void)
{

    struct mpu6xxx_device *dev;
    struct mpu6xxx_3axes accel, gyro, temp;
    eulerian_angles_t e_angles;

    dev = mpu6xxx_init(MPU6XXX_DEVICE_NAME, RT_NULL);

    mpu6xxx_set_param(dev, MPU6XXX_ACCEL_RANGE, MPU6XXX_ACCEL_RANGE_2G);
    mpu6xxx_set_param(dev, MPU6XXX_GYRO_RANGE, MPU6XXX_GYRO_RANGE_250DPS);

    mpu6xxx_set_param(dev, MPU6XXX_DLPF_CONFIG, MPU6XXX_DLPF_188HZ);
    mpu6xxx_set_param(dev, MPU6XXX_SAMPLE_RATE, 1000);

    AccelerationData acceleration;
    FilterBuffer filterBuffer;

    init_acceleration(&acceleration);
    init_filter_buffer(&filterBuffer);

    bool isFirstSample = true;

    float adx, ady, adz;

    while (1)
    {

        mpu6xxx_get_accel(dev, &accel);
        mpu6xxx_get_gyro(dev, &gyro);

        acceleration.ax = accel.x / 100.0f;
        acceleration.ay = accel.y / 100.0f;
        acceleration.az = accel.z / 100.0f;
        acceleration.gx = gyro.x / 100.0f;
        acceleration.gy = gyro.y / 100.0f;
        acceleration.gz = gyro.z / 100.0f;

        decompose_acceleration(adx, ady, adz, &acceleration, isFirstSample);
        isFirstSample = false;

        // 对线性加速度进行预处理
        acceleration.ax = preprocess_data(acceleration.ax, &filterBuffer);
        acceleration.ay = preprocess_data(acceleration.ay, &filterBuffer);
        acceleration.az = preprocess_data(acceleration.az, &filterBuffer);

        // 在这里你可以添加更多的处理逻辑，比如显示数据或保存数据
        printf(" gx %.3f,  gy %.3f,  gz %.3f\r", acceleration.ax, acceleration.ay, acceleration.az);

        // 假设 delay() 是一个延时函数，确保采样频率不超过1000Hz
        rt_thread_mdelay(5); // 这里延迟1毫秒，根据你的采样率进行调整
    }

    return 0;
}
